#include "stm32f10x.h" // Device header
#include "Delay.h"
#include "OLED.h"
#include "PWM.h"
#include "Motor.h"
#include "Serial.h"
#include "Sonic.h"
#include "Servo.h"

uint8_t speed = 40;
uint8_t RxData, left, right;
float dis;
int int_length,decimal_lenght;
int i;

void send()
{
		//前进
		if(RxData == 0x38)
		{
			i = 1;
			RxData = 0;
		}
		
		//后退
		if(RxData == 0x32)
		{
			i = 2;
			RxData = 0;
		}
		
		//左转
		if(RxData == 0x34)
		{
			i = 3;
			RxData = 0;
		}
		
		//右转
		if(RxData == 0x36)
		{
			i = 4;
			RxData = 0;
		}
		
		//停止
		if(RxData == 0x35)
		{
			i = 5;
			RxData = 0;
		}
		
		//超声波
		if(RxData == 0x78)
		{
			i = 6;
			RxData = 0;
		}
}

void mode()
{
		//前进
		if(i == 1)
		{
			Run();
			OLED_ShowString(1, 1, "run  ");
		}
		
		//后退
		if(i == 2)
		{
			Back();
			OLED_ShowString(1, 1, "back ");
		}
		
		//左转
		if(i == 3)
		{
			Left();
			Delay_ms(800);
			OLED_ShowString(1, 1, "left ");
			i = 5;
		}
		
		//右转
		if(i == 4)
		{
			Right();
			Delay_ms(800);
			OLED_ShowString(1, 1, "right");
			i = 5;
		}
		
		//停止
		if(i == 5)
		{
			Stop();
			OLED_ShowString(1, 1, "stop ");
		}
		
		//超声波避障
		if(i == 6)
		{			
			int_length = (uint16_t)dis;//小数点前的数字
			decimal_lenght = dis * 1000 - int_length * 1000;//小数点后的数字

			OLED_ShowNum(3, 1, int_length, 2);
			OLED_ShowString(3, 3, ".");
			OLED_ShowNum(3, 4, decimal_lenght, 3);
			
			if(int_length > 30)
			{
				Run();
			}
			else if(int_length <= 30)
			{
				Back();
				Delay_s(1);
				Stop();
				Delay_s(1);
				int len1,len2;
				Servo_SetAngle(150);
				dis = Sonic_Distance();
				len1 = (uint16_t)dis;//右边距离
				Delay_ms(1);
				Servo_SetAngle(30);
				dis = Sonic_Distance();
				len2 = (uint16_t)dis;//左边距离
				Delay_ms(1);
				
				if(len1 > len2)
				{
					Right();
				}
				else
				{
					Left();
				}
				Servo_SetAngle(80);
			}
		}
}

int main(void)
{
	OLED_Init();
	Motor_Init();//电机
	Servo_Init();//舵机
	Serial_Init();//蓝牙串口
	Sonic_Init();//超声波
	Infrared_Init();//红外避障
	
	//初始速度为40，舵机转动角度为80
	PWM_SetCompare(speed);
	OLED_ShowString(1, 1, "start");
	OLED_ShowString(2, 1, "Speed:");
	OLED_ShowNum(2, 7, speed, 3);
	Servo_SetAngle(80);
	while(1)
	{
		left = Infrared_Status_left();
		right = Infrared_Status_right();
		dis = Sonic_Distance();//超声波测距
		//读取发送来的信号
		if(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == SET)
		{
			RxData = USART_ReceiveData(USART1);
			send();
		}
		mode();
		if(left == 1)
		{
			Back();
			Delay_s(1);
			Right();
			Delay_s(1);
			Stop();
			left = 0;
		}
		if(right == 1)
		{
			Back();
			Delay_s(1);
			Left();
			Delay_s(1);
			Stop();
			right = 0;
		}
		if(left == 1 && right == 1)
		{
			Back();
			Delay_s(1);
			Stop();
			left = 0;
			right = 0;
		}
		//速度增大
		if(RxData == 0x31 && speed != 100)
		{
			speed += 20;
			PWM_SetCompare(speed);
			RxData = 0;
			OLED_ShowNum(2,7, speed, 3);
		}
		
		//速度减小
		if(RxData == 0x33 && speed != 0)
		{
			speed -= 20;
			PWM_SetCompare(speed);
			RxData = 0;
			OLED_ShowNum(2,7, speed, 3);
		}
		
	}
}